#include "modules/CtrlModule/ActivePlanner/ZigzagPlanner.h"
#include "modules/CtrlModule/ActivePlanner/NaviPlanner.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
ZigzagPlanner zigzag_planner;

void ZigzagPlanner::SetNode(const rclcpp::Node::SharedPtr &ros_node, std::shared_ptr<IpaRoomPlanning> &ipa_room_plan_)
{
    ros_node_ = ros_node;
    ipa_room_plan = ipa_room_plan_;
    if (clean_service_ == nullptr)
        clean_service_ = ros_node_->create_service<sweeping_robot_msg::srv::CleanNext>(
                "/clean_next",
                std::bind(&ZigzagPlanner::cleanCallback, this,
                        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}

void ZigzagPlanner::cleanCallback(
        const std::shared_ptr<rmw_request_id_t>/*request_header*/,
        const std::shared_ptr<sweeping_robot_msg::srv::CleanNext::Request> req,
        std::shared_ptr<sweeping_robot_msg::srv::CleanNext::Response> res)
{
    request = req;
    std::cout << "clean_action: " << request->clean_action
              << " FORCE_NEXT_SECTION: " << request->FORCE_NEXT_SECTION << std::endl;
    if (req->clean_action == req->FORCE_NEXT_SECTION)
    {
        request->clean_action = request->NEXT_SECTION;
        if (DataSet::ctrl_state == NAVI_ACTION && DataSet::ctrl_state_sub == NAVI_TO_POSE_PLAN)
        {
            navi_planner.cancelGoal();
        }
        updateState(NAVI_MODE, NAVI_ACTION, COVERAGE_PLAN,
                    NAVI_MODE, NAVI_ACTION, COVERAGE_PLAN);
    }
    res->action_result = true;
}

bool ZigzagPlanner::finishSection()
{
    if (request != nullptr)
    {
        std::cout << "manual clean_action= " << request->clean_action << std::endl;
        if (request->clean_action == request->NEXT_SECTION)
        {
            request = nullptr;
            return true;
        }
        else if (request->clean_action == request->NEXT_ROOM)
        {
            request = nullptr;
            section_id = max_section_id;
            return true;
        }
        else if (request->clean_action == request->LAST_SECTION)
        {
            if (section_id == max_section_id)
            {
                request = nullptr;
                return false;
            }
            else
            {
                request = nullptr;
                section_id = max_section_id - 1;
                return true;
            }
        }
        else if (request->clean_action == request->LAST_ROOM)
        {
            if (ipa_room_plan->roomID() == ipa_room_plan->maxRoom())
            {
                request = nullptr;
                return false;
            }
            else
            {
                request = nullptr;
                section_id = max_section_id;
                ipa_room_plan->roomID() = ipa_room_plan->maxRoom() - 1;
                return true;
            }
        }
        else if (request->clean_action == request->FINISH_ALL)
        {
            ipa_room_plan->roomID() = ipa_room_plan->maxRoom();
            section_id = max_section_id;
            request = nullptr;
            return true;
        }
        request = nullptr;
        return false;
    }
    return false;
}

bool ZigzagPlanner::checkDistance(geometry_msgs::msg::PoseStamped &check_pose, double dis_sqr)
{
    geometry_msgs::msg::PoseStamped robotpose = getRobotPose();
    if (dis_sqr <= 0) dis_sqr = ipa_room_plan->pathEpsSqr();
    return pow(robotpose.pose.position.x - check_pose.pose.position.x, 2) +
           pow(robotpose.pose.position.y - check_pose.pose.position.y, 2) <= dis_sqr;
}

bool ZigzagPlanner::addRobotPose(std::vector<geometry_msgs::msg::PoseStamped> &poses,
                                 std::vector<geometry_msgs::msg::PoseStamped> &poses_sub)
{
    if (!checkDistance(poses_sub[0])) return false;
    geometry_msgs::msg::PoseStamped PoseStamped_tmp;
    Eigen::Quaterniond quaternion;
    poses.push_back(getRobotPose());
    tf2::Quaternion tf_q;
    tf_q.setX(poses[0].pose.orientation.x);
    tf_q.setY(poses[0].pose.orientation.y);
    tf_q.setZ(poses[0].pose.orientation.z);
    tf_q.setW(poses[0].pose.orientation.w);
    double yaw1 = tf2::getYaw(tf_q);
    tf_q.setX(poses_sub[0].pose.orientation.x);
    tf_q.setY(poses_sub[0].pose.orientation.y);
    tf_q.setZ(poses_sub[0].pose.orientation.z);
    tf_q.setW(poses_sub[0].pose.orientation.w);
    double yaw3 = tf2::getYaw(tf_q);
//        double  yaw1 = tf2::getYaw(poses[0].pose.orientation);
//        double  yaw3 = tf2::getYaw(poses_sub[0].pose.orientation);
    double yaw2 = atan2(poses_sub[0].pose.position.y - poses[0].pose.position.y,
                        poses_sub[0].pose.position.x - poses[0].pose.position.x);
    quaternion = Eigen::AngleAxisd(yaw2, Eigen::Vector3d::UnitZ());
    if (fabs(yaw1 - yaw2) > 0.052359876)  // 3度
    {
        PoseStamped_tmp = poses[0];
        PoseStamped_tmp.pose.orientation.set__x(quaternion.x());
        PoseStamped_tmp.pose.orientation.set__y(quaternion.y());
        PoseStamped_tmp.pose.orientation.set__z(quaternion.z());
        PoseStamped_tmp.pose.orientation.set__w(quaternion.w());
        poses.push_back(PoseStamped_tmp);
    }
    if (fabs(yaw2 - yaw3) > 0.052359876)  // 3度
    {
        PoseStamped_tmp = poses_sub[0];
        PoseStamped_tmp.pose.orientation.set__x(quaternion.x());
        PoseStamped_tmp.pose.orientation.set__y(quaternion.y());
        PoseStamped_tmp.pose.orientation.set__z(quaternion.z());
        PoseStamped_tmp.pose.orientation.set__w(quaternion.w());
        poses.push_back(PoseStamped_tmp);
    }
    poses.insert(poses.end(), poses_sub.begin(), poses_sub.end());
    return true;
}

bool ZigzagPlanner::hasRepairLeak()
{
    return false;
}

void ZigzagPlanner::run(int &finished_id, double &linear_vel, double &angular_vel,
                        geometry_msgs::msg::PoseStamped &goal_pose)
{ //finished_id:0 no finish,  1 finish one obstacle point , 2 finish one section, 3 finish one room. 4 finish all
    if (!Plan())
    {
        std::cout << "ipa plan failed !" << std::endl;
        finished_id = FINISHED;
        return;
    }
    if (ipa_room_plan->maxRoom() < 1)
    {
        std::cout << "no room!" << std::endl;
        finished_id = FINISHED;
        return;
    }
    goal_pose.header.frame_id = "map";
    if (ipa_room_plan->roomID() < ipa_room_plan->maxRoom() && done_prepare)
    {
        auto &poses = ipa_room_plan->result_exp->coverage_path_pose_stamped;
        if (room_ready)
        {
            std::cout << "room_ready" << std::endl;
            ipa_room_plan->cutOff(poses, cut_off_ids);
            max_section_id = cut_off_ids.size() / 2;
            section_id = 0;
            room_ready = false;
            section_ready = true;
        }

        if (section_id < max_section_id && section_id > -1)
        {
            if (section_ready)
            {
                std::cout << "section_ready" << std::endl;
                plan_poses_ori.clear();
                plan_poses_set.clear();
                plan_poses_ori.insert(plan_poses_ori.end(), poses.begin() + cut_off_ids[section_id * 2],
                                      poses.begin() + cut_off_ids[section_id * 2 + 1] + 1);
                if (!addRobotPose(plan_poses_set, plan_poses_ori))
                {
                    finished_id = OVER_SECTION;
                    goal_pose = plan_poses_ori[0];
                    goal_pose.header.stamp = ros_node_->now();
                    last_goal_pose = goal_pose;
                    return;
                }
                ipa_room_plan->setPlan(plan_poses_set);
                ipa_room_plan->sectionPublish(plan_poses_set);
                section_ready = false;
                section_id++;
            }
        }

        if (!finishSection() && ipa_room_plan->Moving(linear_vel, angular_vel))
        {
            auto position_tmp = ipa_room_plan->goalPose(false, false).pose.position;
            if ((pow(position_tmp.x, 2) + pow(position_tmp.y, 2)) >= 0.36 &&
                fabs(position_tmp.y) > 0.424264069)  //0.6*0.6; 0.6*sqre(2)*0.5*0.424264069
            {
                finished_id = OVER_SECTION;
                goal_pose = ipa_room_plan->goalPose(true, false);
                goal_pose.header.stamp = ros_node_->now();
                last_goal_pose = goal_pose;
                std::cout << "robot lost goal, try return goal..." << std::endl;
                return;
            }
            finished_id = UNFINISHED;
        }
        else
        {
            std::cout << "move: FINISHED!" << std::endl;
            if (section_id >= max_section_id)
            {
                ipa_room_plan->roomID()++;
                poses.clear();
                while (ipa_room_plan->roomID() < ipa_room_plan->maxRoom() && rclcpp::ok())
                {
                    if (ipa_room_plan->roomExploration(ipa_room_plan->roomID()) > 0)
                        break;
                    else
                        ipa_room_plan->roomID()++;
                }
                auto &poses_new = ipa_room_plan->result_exp->coverage_path_pose_stamped;
                if (!poses_new.empty())
                {
                    goal_pose = poses_new[0];
                    goal_pose.header.stamp = ros_node_->now();
                    last_goal_pose = goal_pose;
                    room_ready = true;
                    finished_id = OVER_ROOM;
                    std::cout << "next goal_pose x= " << goal_pose.pose.position.x << " y= "
                              << goal_pose.pose.position.y << std::endl;
                } else finished_id = FINISHED;
            } else
            {
                section_ready = true;
                goal_pose = poses[cut_off_ids[section_id * 2]];
                goal_pose.header.stamp = ros_node_->now();
                last_goal_pose = goal_pose;
                finished_id = OVER_SECTION;
            }
        }
    }
    else finished_id = FINISHED;
}

bool ZigzagPlanner::addRooms(std::vector<std::vector<cv::Point> > &contours)
{
    ipa_building_msgs::msg::RoomInformation information;
    information.room_min_max.points.resize(2);
    for (int j = 0; j < contours.size(); j++)
    {
        double x = 0, y = 0;
        int min_x = 10000, min_y = 10000, max_x = -10000, max_y = -10000;
        for (auto & i : contours[j])
        {
            x += i.x;
            y += i.y;
            min_x = std::min(min_x, i.x);
            max_x = std::max(max_x, i.x);
            min_y = std::min(min_y, i.y);
            max_y = std::max(max_y, i.y);
        }
        information.room_min_max.points[0].x = min_x;
        information.room_min_max.points[0].y = min_y;
        information.room_min_max.points[1].x = max_x;
        information.room_min_max.points[1].y = max_y;
        information.room_center.x = x / contours[j].size();
        information.room_center.y = y / contours[j].size();
        int room_num = ipa_room_plan->result_seg->room_information_in_pixel.size();
        ipa_room_plan->Sequence().push_back(room_num);
        cv::drawContours(ipa_room_plan->exploration_map, contours, j,
                cv::Scalar(room_num + 1), -1);
        ipa_room_plan->result_seg->room_information_in_pixel.push_back(information);
    }
    if (section_id >= max_section_id && ipa_room_plan->roomID() >= ipa_room_plan->maxRoom())
    {
        room_ready = true;
        ipa_room_plan->maxRoom() = ipa_room_plan->result_seg->room_information_in_pixel.size();
        while (ipa_room_plan->roomID() < ipa_room_plan->maxRoom() && rclcpp::ok())
        {
            if (ipa_room_plan->roomExploration(ipa_room_plan->roomID()) > 0)
                break;
            else ipa_room_plan->roomID()++;
        }
        updateState(NAVI_MODE, NAVI_ACTION, COVERAGE_PLAN,
                    NAVI_MODE, NAVI_ACTION, COVERAGE_PLAN);
    }
    else
    {
        ipa_room_plan->maxRoom() = ipa_room_plan->result_seg->room_information_in_pixel.size();
    }
}

bool ZigzagPlanner::Plan()
{
    if (ipa_room_plan == nullptr) return false;
    if (done_prepare) return true;
    if (ipa_room_plan->dataDone() && ipa_room_plan->roomSegmentationDone())
    {
        std::cout << "maxRoom= " << ipa_room_plan->maxRoom() << std::endl;
        if (!ipa_room_plan->roomSequenceDone()) return false;
        done_prepare = true;
    }
    return done_prepare;
}

}
